Summary (Moog theory)

Observabilidad

Ver ejemplo pagina 64.

Caso SISO en general es sacar las $y $ y sus derivadas y derivar parcialmente entre x, ver el rango de la matrix que se crea

Caso MIMO?

Full linearzation via Static state feedback (general):

Solvable conditions:

In general full static stae feedback linearization is solvable if and only if:

$H_ \infty $ = {0}
And all subspaces $H_ k$ are integrable

The last point means that:

\begin{equation} \begin{matrix} H_1 = \omega_1 \text{must be integrable}\\ H_2 = \omega_2 \text{must be integrable}\\ \cdots H_k = \omega_k \text{must be integrable} \end{matrix} \end{equation}

For this we can chech with:

http://www.nlcontrol.ioc.ee/webmathematica/NLControl/main/continuous/SequenceH.html

And example:

{Cos[x3[t]] u1[t], Sin[x3[t]]u1[t] , Tan[x4[t]]u1[t]/L, u2[t]}

{x1[t], x2[t],x3[t],x4[t]}

{u1[t], u2[t]}

Input output linearization

Description:

p es la dimención del vector y = h(x)

SISO case (Solo una $y$ y una $u$):

Then the static state feedback input-output linearization problem is solvable if and only if its relative degree is finite.

if $r_d < n$, unbservable subsytem can be unestable, if $r_d == n$ no hay unobservable subsytem (no problem)

Multioutput Case

obtener y y sus derivadas y crear la decopling matrix y si es menor a p(vector de entrada) entonces no es linealirizable by ssf

Input / State linarization

Static state feedback

Ventajas

Allows internal stability
No zero dynamics
No internal unstability

Solvable conditions (given the outputs y):

Only if the sum of the relative degrees are equal to $n$.
Where $n$ is the dimension of the state space.

Solvable conditions (without outputs y):

$H_ \infty $ = {0}
And all subspaces $H_ k$ are integrable

Non interacting control

Static state feedback (non interacting control):

De las entradas derivar y sacar $y, \dot{y}, ...$ hasta que aparescan las salidas, formar la mtrix (que no es la decopling matrix, esta mal la figura, solo juntar los ementos) y ver el rango.

Ver ejemplo pag 93 apuntes

Para resolverlo, igual De las entradas derivar y sacar $y, \dot{y}, ...$ hasta que aparescan las salidas, formar D(x).

hacer:

$y_1^{(r_1)} = v_1$,.... $y_k^{(r_k)} = v_k$, para $k = 1, ..., p$, donde $p$ es la dimension de vector de salida, donde $r_n$ son lo relative degree (cuantas veces se integro para obtener la salida deseada), y $v_k$ los elementos de las nuevas entrada

Dynamic state feedback (non interacting control):

The noninteracting control problem is solvable via regular dynamic state feedback if and

Ver ejemplo pag 99 apuntes

only if the system is right invertible.



In [1]:

    
from sympy.abc import*
from sympy import *
from tools import*
from nlcontrol import*

var = ['x_1', 'x_2', 'x_3', 'x_4','x_5','x_6']
x1,x2,x3,x4,x5,x6 = def_states('x',6,False)
u1,u2,u3,u4,u5,u6 = def_states('u',6,False)

g1= Matrix([cos(x3),sin(x3),tan(x4), 0])
g2= Matrix([0, 0, 0, 1])



In [ ]:

State elimination, i/o injection, tambien static state feedback, observabilidad

Ejemplo 1

State elimination



In [45]:

    
from tools import*
from nlcontrol import*
x1,x2,x3,x4,x5,x6 = def_states('x',6,True)
u1,u2,u3 = def_states('u',3,True)
a = Symbol('a')

x1d = x2
x2d = x1*x2 + a*sin(x1)*u1 
x3d = []
x4d = []
x5d = []
x6d = []

y = x1 # hx

#State equation
states_dot = [x1d,x2d]
states = [x1,x2]

#State elimination
yd = states_cancel(y, states, states_dot)
ydd = states_cancel(yd, states, states_dot)
ydd









    Out[45]:





$$a \operatorname{u_{1}}{\left (t \right )} \sin{\left (\operatorname{x_{1}}{\left (t \right )} \right )} + \operatorname{x_{1}}{\left (t \right )} \operatorname{x_{2}}{\left (t \right )}$$

Observabilidad



In [46]:

    
#Observabilidad, para ello sacar el jacobiano de las y's hasta n-1 (state dimension)
# y obtener el rango

ys = [y,yd] #Hasta n-1
ys = Matrix(ys)
J = jacobian(ys,states)
J









    Out[46]:





$$\left[\begin{matrix}1 & 0\\0 & 1\end{matrix}\right]$$



In [47]:

    
#Obtener el rango, must be == n para ser full observable
J.rank()









    Out[47]:





$$2$$

Input output injection



In [50]:

    
#Ultima derivada para input/output injection
yn = ydd
yn









    Out[50]:





$$a \operatorname{u_{1}}{\left (t \right )} \sin{\left (\operatorname{x_{1}}{\left (t \right )} \right )} + \operatorname{x_{1}}{\left (t \right )} \operatorname{x_{2}}{\left (t \right )}$$



In [51]:

    
#y para susbtituir
# Definir otra vez y que ya estaba , si no da error  <importante
y, = def_vars(['y'],True)
ysd,ysdd = def_state_der('y',2,True)

#y para sustutuir, calculado a mano :(
xs1 = y
xs2 = ysd

vector_xs = [x1,x2]  #Hasta n
vector_ys = [xs1,xs2] #Hasta n

k = 0
for var in vector_xs:
    yn = yn.subs(var, vector_ys[k])
    k = k +1
yn









    Out[51]:





$$a \operatorname{u_{1}}{\left (t \right )} \sin{\left (y{\left (t \right )} \right )} + y{\left (t \right )} \operatorname{y^{{(1)}}}{\left (t \right )}$$



In [82]:

    
#Input output injection
Py = Matrix([yn]) #Ya solo con respecto a y y derivadas e u y derivadas
# if dw != 0 stop
n = 2 #Number o state vectors <---- ojo cambiar 
yd,ydd = def_state_der('y', n ,True) #<-- esta tambien y correr desde el pricipio
ud,udd = def_state_der('u', n ,True) #<-- esta tambien y correr desde el pricipio
y, = def_vars(['y'],True)
u, = def_vars(['u'],True)

ys_derivar = [yd,y] # De y^(n-1) a yvarf = ['dy', 'du']
us_derivar = [ud,u] # De y^(n-1) a yvarf = ['dy', 'du']

# 1 obtener parcial de py con respecto a las y(n-i), i = 1,...n  por dy
varf = ['y', 'u'] #Solo simbolos se agregan solas la y
dy,du =  def_1forms(varf)

w_ypart = jacobian(Py,[ys_derivar[0]]) #Pasar como lista
w_upart = jacobian(Py,[us_derivar[0]])
w = w_ypart*dy + w_upart*du
w

var  = ['y','u'] #Esta creo no se cambia, casi seguro que no

dw = ediff(w,var)
dw

if (dw[0] == 0):
    print "integrable, no stop"

#Lo demas haslo a mano pag 77,78,79 de las notas, checa que son sumatorias de u's
# tambien el cambio de variable es sin sustituir x's o y's tiene su forma que hace canonica obsrervable, 
    
#Ejemplo de integral
y = Symbol('y')
function = y
#(diferencial dy, desde 0, a y)
integrate(function, (y,0,y))









    



integrable, no stop






    Out[82]:





$$\frac{y^{2}}{2}$$

Ejemplo 2

States elimination



In [3]:

    
#from sympy.abc import*
#from sympy import *
#from tools import*
#from nlcontrol import*



In [14]:

    
from tools import*
x1,x2,x3,x4,x5,x6 = def_states('x',6,True)
u1,u2,u3 = def_states('u',3,True)
a = Symbol('a')

#State definition
x1d = x2
x2d = -x2 -sin(x1) - x1 + x3
x3d = x4
x4d = -x4 + x1 -x3 + u1

#Output
y = x1 # hx

#State equation
states_dot = [x1d,x2d,x3d,x4d]
states = [x1,x2,x3,x4]

yd = states_cancel(y, states, states_dot)
yd









    Out[14]:





$$\operatorname{x_{2}}{\left (t \right )}$$



In [15]:

    
ydd = states_cancel(yd, states, states_dot)
ydd









    Out[15]:





$$- \operatorname{x_{1}}{\left (t \right )} - \operatorname{x_{2}}{\left (t \right )} + \operatorname{x_{3}}{\left (t \right )} - \sin{\left (\operatorname{x_{1}}{\left (t \right )} \right )}$$



In [16]:

    
yddd = states_cancel(ydd, states, states_dot)
yddd









    Out[16]:





$$\operatorname{x_{1}}{\left (t \right )} - \operatorname{x_{2}}{\left (t \right )} \cos{\left (\operatorname{x_{1}}{\left (t \right )} \right )} - \operatorname{x_{3}}{\left (t \right )} + \operatorname{x_{4}}{\left (t \right )} + \sin{\left (\operatorname{x_{1}}{\left (t \right )} \right )}$$



In [17]:

    
ydddd = states_cancel(yddd, states, states_dot)
ydddd









    Out[17]:





$$- \left(- \operatorname{x_{1}}{\left (t \right )} - \operatorname{x_{2}}{\left (t \right )} + \operatorname{x_{3}}{\left (t \right )} - \sin{\left (\operatorname{x_{1}}{\left (t \right )} \right )}\right) \cos{\left (\operatorname{x_{1}}{\left (t \right )} \right )} + \operatorname{u_{1}}{\left (t \right )} + \operatorname{x_{1}}{\left (t \right )} + \operatorname{x_{2}}^{2}{\left (t \right )} \sin{\left (\operatorname{x_{1}}{\left (t \right )} \right )} + \operatorname{x_{2}}{\left (t \right )} \cos{\left (\operatorname{x_{1}}{\left (t \right )} \right )} + \operatorname{x_{2}}{\left (t \right )} - \operatorname{x_{3}}{\left (t \right )} - 2 \operatorname{x_{4}}{\left (t \right )}$$



In [18]:

    
simplify(ydddd)









    Out[18]:





$$\left(\operatorname{x_{1}}{\left (t \right )} + \operatorname{x_{2}}{\left (t \right )} - \operatorname{x_{3}}{\left (t \right )} + \sin{\left (\operatorname{x_{1}}{\left (t \right )} \right )}\right) \cos{\left (\operatorname{x_{1}}{\left (t \right )} \right )} + \operatorname{u_{1}}{\left (t \right )} + \operatorname{x_{1}}{\left (t \right )} + \operatorname{x_{2}}^{2}{\left (t \right )} \sin{\left (\operatorname{x_{1}}{\left (t \right )} \right )} + \operatorname{x_{2}}{\left (t \right )} \cos{\left (\operatorname{x_{1}}{\left (t \right )} \right )} + \operatorname{x_{2}}{\left (t \right )} - \operatorname{x_{3}}{\left (t \right )} - 2 \operatorname{x_{4}}{\left (t \right )}$$

Observabilidad



In [19]:

    
#Observabilidad, para ello sacar el jacobiano de las y's hasta n-1 (state dimension)
# y obtener el rango

ys = [y,yd,ydd,yddd] #Hasta n-1
ys = Matrix(ys)
J = jacobian(ys,states)
J









    Out[19]:





$$\left[\begin{matrix}1 & 0 & 0 & 0\\0 & 1 & 0 & 0\\- \cos{\left (\operatorname{x_{1}}{\left (t \right )} \right )} - 1 & -1 & 1 & 0\\\operatorname{x_{2}}{\left (t \right )} \sin{\left (\operatorname{x_{1}}{\left (t \right )} \right )} + \cos{\left (\operatorname{x_{1}}{\left (t \right )} \right )} + 1 & - \cos{\left (\operatorname{x_{1}}{\left (t \right )} \right )} & -1 & 1\end{matrix}\right]$$



In [20]:

    
#Obtener el rango
J.rank()









    Out[20]:





$$4$$

Static state feedback



In [ ]:

    
v = ydddd

Dynamic feedback



In [142]:

    
from tools import*
from nlcontrol import*

x1,x2,x3,x4,x5,x6 = def_states('x',6,True)
u1,u2,u3 = def_states('u',3,True)
y1ds,y1dds,y1ddds = def_state_der('y_1', 3 ,True) #<-- esta tambien y correr desde el pricipio

a = Symbol('a')
L = Symbol('L')

#State definition
x1d = u1*cos(x3)
x2d = u1*sin(x3)
x3d = u2

#Output
y1 = x1 # hx

#State equation
states_dot = [x1d,x2d,x3d,x4d]
states = [x1,x2,x3,x4]

y1d = states_cancel(y1, states, states_dot)
y1d









    Out[142]:





$$\operatorname{u_{1}}{\left (t \right )} \cos{\left (\operatorname{x_{3}}{\left (t \right )} \right )}$$



In [143]:

    
#Output
y2 = x2 # hx

#State equation
states_dot = [x1d,x2d,x3d,x4d]
states = [x1,x2,x3,x4]

y2d = states_cancel(y2, states, states_dot)
y2d









    Out[143]:





$$\operatorname{u_{1}}{\left (t \right )} \sin{\left (\operatorname{x_{3}}{\left (t \right )} \right )}$$



In [144]:

    
y2ds,y2dds,y2ddds = def_state_der('y_2', 3 ,True) #<-- esta tambien y correr desde el pricipio
u1s = solve(y1d - y1ds, u1 )
u1s









    Out[144]:





$$\left [ \frac{\operatorname{y^{{(1)}}_{1}}{\left (t \right )}}{\cos{\left (\operatorname{x_{3}}{\left (t \right )} \right )}}\right ]$$



In [145]:

    
y2d = simplify(y2d.subs(u1,u1s[0]))
y2d









    Out[145]:





$$\operatorname{y^{{(1)}}_{1}}{\left (t \right )} \tan{\left (\operatorname{x_{3}}{\left (t \right )} \right )}$$



In [146]:

    
y2dd = trigsimp(states_cancel(y2d, states, states_dot))
y2dd









    Out[146]:





$$\frac{\operatorname{u_{2}}{\left (t \right )} \operatorname{y^{{(1)}}_{1}}{\left (t \right )}}{\cos^{2}{\left (\operatorname{x_{3}}{\left (t \right )} \right )}} + \tan{\left (\operatorname{x_{3}}{\left (t \right )} \right )} \frac{d}{d t} \operatorname{y^{{(1)}}_{1}}{\left (t \right )}$$



In [147]:

    
ysvar = [y1d,y2dd ]
ys = Matrix([ysvar])
ys









    Out[147]:





$$\left[\begin{matrix}\operatorname{u_{1}}{\left (t \right )} \cos{\left (\operatorname{x_{3}}{\left (t \right )} \right )} & \frac{\operatorname{u_{2}}{\left (t \right )} \operatorname{y^{{(1)}}_{1}}{\left (t \right )}}{\cos^{2}{\left (\operatorname{x_{3}}{\left (t \right )} \right )}} + \tan{\left (\operatorname{x_{3}}{\left (t \right )} \right )} \frac{d}{d t} \operatorname{y^{{(1)}}_{1}}{\left (t \right )}\end{matrix}\right]$$



In [148]:

    
uvar = [u1, u2]
J = jacobian(ys,uvar)
J









    Out[148]:





$$\left[\begin{matrix}\cos{\left (\operatorname{x_{3}}{\left (t \right )} \right )} & 0\\0 & \frac{\operatorname{y^{{(1)}}_{1}}{\left (t \right )}}{\cos^{2}{\left (\operatorname{x_{3}}{\left (t \right )} \right )}}\end{matrix}\right]$$



In [149]:

    
J.rank()









    Out[149]:





$$2$$



In [150]:

    
Dx = J
bx = Matrix([0,tan(x3)*y1d])

v1,v2,v3,v4 = symbols('v_1 v_2 v_3 v_4',cls=Function)
z1,z2,z3,z4 = symbols('z_1 z_2 z_3 z_4',cls=Function)

V = Matrix(([z1(t)],[v2(t)]))
V









    Out[150]:





$$\left[\begin{matrix}\operatorname{z_{1}}{\left (t \right )}\\\operatorname{v_{2}}{\left (t \right )}\end{matrix}\right]$$



In [151]:

    
def sfeed(Dx, bx, V):
    betax = Dx**-1*V
    alfax = Dx**-1*bx
    return simplify(alfax + betax)



In [152]:

    
sfeed(Dx, bx, V)









    Out[152]:





$$\left[\begin{matrix}\frac{\operatorname{z_{1}}{\left (t \right )}}{\cos{\left (\operatorname{x_{3}}{\left (t \right )} \right )}}\\\frac{\cos^{2}{\left (\operatorname{x_{3}}{\left (t \right )} \right )}}{\operatorname{y^{{(1)}}_{1}}{\left (t \right )}} \left(\operatorname{u_{1}}{\left (t \right )} \sin{\left (\operatorname{x_{3}}{\left (t \right )} \right )} + \operatorname{v_{2}}{\left (t \right )}\right)\end{matrix}\right]$$

Ejemplo examen



In [153]:

    
#State definition
x1d = u1*cos(x3)
x2d = u1*sin(x3)
x3d = u1*tan(x4)/L
x4d = u2

#Output
y1 = x1 # hx

#State equation
states_dot = [x1d,x2d,x3d,x4d]
states = [x1,x2,x3,x4]

y1d = states_cancel(y1, states, states_dot)
y1d









    Out[153]:





$$\operatorname{u_{1}}{\left (t \right )} \cos{\left (\operatorname{x_{3}}{\left (t \right )} \right )}$$



In [154]:

    
#Output
y2 = x2 # hx

#State equation
states_dot = [x1d,x2d,x3d,x4d]
states = [x1,x2,x3,x4]

y2d = states_cancel(y2, states, states_dot)
y2d









    Out[154]:





$$\operatorname{u_{1}}{\left (t \right )} \sin{\left (\operatorname{x_{3}}{\left (t \right )} \right )}$$



In [156]:

    
y1ds,y1dds,y1ddds = def_state_der('y_1', 3 ,True) #<-- esta tambien y correr desde el pricipio
y2ds,y2dds,y2ddds = def_state_der('y_2', 3 ,True) #<-- esta tambien y correr desde el pricipio
u1s = solve(y1d - y1ds, u1 )
u1s









    Out[156]:





$$\left [ \frac{\operatorname{y^{{(1)}}_{1}}{\left (t \right )}}{\cos{\left (\operatorname{x_{3}}{\left (t \right )} \right )}}\right ]$$



In [157]:

    
y2d = simplify(y2d.subs(u1,u1s[0]))
y2d









    Out[157]:





$$\operatorname{y^{{(1)}}_{1}}{\left (t \right )} \tan{\left (\operatorname{x_{3}}{\left (t \right )} \right )}$$



In [158]:

    
y2dd = trigsimp(states_cancel(y2d, states, states_dot))
y2dd









    Out[158]:





$$\tan{\left (\operatorname{x_{3}}{\left (t \right )} \right )} \frac{d}{d t} \operatorname{y^{{(1)}}_{1}}{\left (t \right )} + \frac{\operatorname{u_{1}}{\left (t \right )} \operatorname{y^{{(1)}}_{1}}{\left (t \right )} \tan{\left (\operatorname{x_{4}}{\left (t \right )} \right )}}{L \cos^{2}{\left (\operatorname{x_{3}}{\left (t \right )} \right )}}$$



In [159]:

    
y2dd = simplify(y2dd.subs(u1,u1s[0]))
y2dd









    Out[159]:





$$\tan{\left (\operatorname{x_{3}}{\left (t \right )} \right )} \frac{d}{d t} \operatorname{y^{{(1)}}_{1}}{\left (t \right )} + \frac{\operatorname{y^{{(1)}}_{1}}^{2}{\left (t \right )} \tan{\left (\operatorname{x_{4}}{\left (t \right )} \right )}}{L \cos^{3}{\left (\operatorname{x_{3}}{\left (t \right )} \right )}}$$



In [160]:

    
y2dd = simplify(y2dd.subs(y1ds**2,y1dds))
y2dd









    Out[160]:





$$\tan{\left (\operatorname{x_{3}}{\left (t \right )} \right )} \frac{d}{d t} \operatorname{y^{{(1)}}_{1}}{\left (t \right )} + \frac{\operatorname{y^{{(2)}}_{1}}{\left (t \right )} \tan{\left (\operatorname{x_{4}}{\left (t \right )} \right )}}{L \cos^{3}{\left (\operatorname{x_{3}}{\left (t \right )} \right )}}$$



In [171]:

    
y2ddd = trigsimp(states_cancel(y2dd, states, states_dot))
y2ddd









    Out[171]:





$$\tan{\left (\operatorname{x_{3}}{\left (t \right )} \right )} \frac{d^{2}}{d t^{2}}  \operatorname{y^{{(1)}}_{1}}{\left (t \right )} + \frac{\tan{\left (\operatorname{x_{4}}{\left (t \right )} \right )} \frac{d}{d t} \operatorname{y^{{(1)}}_{1}}{\left (t \right )}}{L \cos^{2}{\left (\operatorname{x_{3}}{\left (t \right )} \right )}} \operatorname{u_{1}}{\left (t \right )} + \frac{\operatorname{u_{2}}{\left (t \right )} \operatorname{y^{{(2)}}_{1}}{\left (t \right )}}{L \cos^{3}{\left (\operatorname{x_{3}}{\left (t \right )} \right )} \cos^{2}{\left (\operatorname{x_{4}}{\left (t \right )} \right )}} + \frac{\tan{\left (\operatorname{x_{4}}{\left (t \right )} \right )} \frac{d}{d t} \operatorname{y^{{(2)}}_{1}}{\left (t \right )}}{L \cos^{3}{\left (\operatorname{x_{3}}{\left (t \right )} \right )}} + \frac{3 \sin{\left (\operatorname{x_{3}}{\left (t \right )} \right )} \tan^{2}{\left (\operatorname{x_{4}}{\left (t \right )} \right )}}{L^{2} \cos^{4}{\left (\operatorname{x_{3}}{\left (t \right )} \right )}} \operatorname{u_{1}}{\left (t \right )} \operatorname{y^{{(2)}}_{1}}{\left (t \right )}$$



In [172]:

    
ysvar = [y1d,y2ddd ]
ys = Matrix([ysvar])
ys









    Out[172]:





$$\left[\begin{matrix}\operatorname{u_{1}}{\left (t \right )} \cos{\left (\operatorname{x_{3}}{\left (t \right )} \right )} & \tan{\left (\operatorname{x_{3}}{\left (t \right )} \right )} \frac{d^{2}}{d t^{2}}  \operatorname{y^{{(1)}}_{1}}{\left (t \right )} + \frac{\tan{\left (\operatorname{x_{4}}{\left (t \right )} \right )} \frac{d}{d t} \operatorname{y^{{(1)}}_{1}}{\left (t \right )}}{L \cos^{2}{\left (\operatorname{x_{3}}{\left (t \right )} \right )}} \operatorname{u_{1}}{\left (t \right )} + \frac{\operatorname{u_{2}}{\left (t \right )} \operatorname{y^{{(2)}}_{1}}{\left (t \right )}}{L \cos^{3}{\left (\operatorname{x_{3}}{\left (t \right )} \right )} \cos^{2}{\left (\operatorname{x_{4}}{\left (t \right )} \right )}} + \frac{\tan{\left (\operatorname{x_{4}}{\left (t \right )} \right )} \frac{d}{d t} \operatorname{y^{{(2)}}_{1}}{\left (t \right )}}{L \cos^{3}{\left (\operatorname{x_{3}}{\left (t \right )} \right )}} + \frac{3 \sin{\left (\operatorname{x_{3}}{\left (t \right )} \right )} \tan^{2}{\left (\operatorname{x_{4}}{\left (t \right )} \right )}}{L^{2} \cos^{4}{\left (\operatorname{x_{3}}{\left (t \right )} \right )}} \operatorname{u_{1}}{\left (t \right )} \operatorname{y^{{(2)}}_{1}}{\left (t \right )}\end{matrix}\right]$$



In [173]:

    
uvar = [u1, u2]
J = jacobian(ys,uvar)
J









    Out[173]:





$$\left[\begin{matrix}\cos{\left (\operatorname{x_{3}}{\left (t \right )} \right )} & 0\\\frac{\tan{\left (\operatorname{x_{4}}{\left (t \right )} \right )} \frac{d}{d t} \operatorname{y^{{(1)}}_{1}}{\left (t \right )}}{L \cos^{2}{\left (\operatorname{x_{3}}{\left (t \right )} \right )}} + \frac{3 \sin{\left (\operatorname{x_{3}}{\left (t \right )} \right )} \tan^{2}{\left (\operatorname{x_{4}}{\left (t \right )} \right )}}{L^{2} \cos^{4}{\left (\operatorname{x_{3}}{\left (t \right )} \right )}} \operatorname{y^{{(2)}}_{1}}{\left (t \right )} & \frac{\operatorname{y^{{(2)}}_{1}}{\left (t \right )}}{L \cos^{3}{\left (\operatorname{x_{3}}{\left (t \right )} \right )} \cos^{2}{\left (\operatorname{x_{4}}{\left (t \right )} \right )}}\end{matrix}\right]$$



In [174]:

    
J.rank()









    Out[174]:





$$2$$



In [175]:

    
Dx = J
bx = Matrix([0,tan(x3)*y1d])

v1,v2,v3,v4 = symbols('v_1 v_2 v_3 v_4',cls=Function)
z1,z2,z3,z4 = symbols('z_1 z_2 z_3 z_4',cls=Function)

V = Matrix(([z1(t)],[v2(t)]))
V









    Out[175]:





$$\left[\begin{matrix}\operatorname{z_{1}}{\left (t \right )}\\\operatorname{v_{2}}{\left (t \right )}\end{matrix}\right]$$



In [176]:

    
def sfeed(Dx, bx, V):
    betax = Dx**-1*V
    alfax = Dx**-1*bx
    return simplify(alfax + betax)



In [177]:

    
sfeed(Dx, bx, V)









    Out[177]:





$$\left[\begin{matrix}\frac{\operatorname{z_{1}}{\left (t \right )}}{\cos{\left (\operatorname{x_{3}}{\left (t \right )} \right )}}\\\frac{\cos^{2}{\left (\operatorname{x_{4}}{\left (t \right )} \right )}}{L \operatorname{y^{{(2)}}_{1}}{\left (t \right )} \cos^{2}{\left (\operatorname{x_{3}}{\left (t \right )} \right )}} \left(L^{2} \left(\operatorname{u_{1}}{\left (t \right )} \sin{\left (\operatorname{x_{3}}{\left (t \right )} \right )} + \operatorname{v_{2}}{\left (t \right )}\right) \cos^{5}{\left (\operatorname{x_{3}}{\left (t \right )} \right )} - \left(L \cos^{2}{\left (\operatorname{x_{3}}{\left (t \right )} \right )} \frac{d}{d t} \operatorname{y^{{(1)}}_{1}}{\left (t \right )} + 3 \operatorname{y^{{(2)}}_{1}}{\left (t \right )} \sin{\left (\operatorname{x_{3}}{\left (t \right )} \right )} \tan{\left (\operatorname{x_{4}}{\left (t \right )} \right )}\right) \operatorname{z_{1}}{\left (t \right )} \tan{\left (\operatorname{x_{4}}{\left (t \right )} \right )}\right)\end{matrix}\right]$$



In [181]:

    
#State definition
x1d = x2
x2d = 1 -x2 - (x3**2)/(1+x1)**2
x3d = (2+x1)/(1+x1)*(-x3 + (x2*x3)/(1+x1**2) +u1)

#Output
y1 = x1 # hx

#State equation
states_dot = [x1d,x2d,x3d]
states = [x1,x2,x3]

y1d = states_cancel(y1, states, states_dot)
y1d









    Out[181]:





$$\operatorname{x_{2}}{\left (t \right )}$$



In [182]:

    
y1dd = states_cancel(y1d, states, states_dot)
y1dd









    Out[182]:





$$- \operatorname{x_{2}}{\left (t \right )} + 1 - \frac{\operatorname{x_{3}}^{2}{\left (t \right )}}{\left(\operatorname{x_{1}}{\left (t \right )} + 1\right)^{2}}$$



In [183]:

    
y1ddd = states_cancel(y1dd, states, states_dot)
y1ddd









    Out[183]:





$$\operatorname{x_{2}}{\left (t \right )} - 1 + \frac{\operatorname{x_{3}}^{2}{\left (t \right )}}{\left(\operatorname{x_{1}}{\left (t \right )} + 1\right)^{2}} - \frac{2 \operatorname{x_{3}}{\left (t \right )}}{\left(\operatorname{x_{1}}{\left (t \right )} + 1\right)^{3}} \left(\operatorname{x_{1}}{\left (t \right )} + 2\right) \left(\operatorname{u_{1}}{\left (t \right )} - \operatorname{x_{3}}{\left (t \right )} + \frac{\operatorname{x_{2}}{\left (t \right )} \operatorname{x_{3}}{\left (t \right )}}{\operatorname{x_{1}}^{2}{\left (t \right )} + 1}\right) + \frac{2 \operatorname{x_{2}}{\left (t \right )} \operatorname{x_{3}}^{2}{\left (t \right )}}{\left(\operatorname{x_{1}}{\left (t \right )} + 1\right)^{3}}$$



In [ ]:

    
solve(a1*(2 + x1) -1 -x1, a1)